{
 "cells": [
  {
   "cell_type": "markdown",
   "id": "9d943c48-c3c6-4a5c-b332-11af6ae79212",
   "metadata": {},
   "source": [
    "# Demo：拖动示教（单个位置）\n",
    "\n",
    "同济子豪兄 2024-5-10"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "e7517ca5-fa3f-4686-ae24-5332afc83379",
   "metadata": {},
   "source": [
    "## 导入工具包"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "id": "3ff223b1-efeb-4c64-bb0e-d75c565ebf6f",
   "metadata": {},
   "outputs": [],
   "source": [
    "from pymycobot.mycobot import MyCobot\n",
    "from pymycobot import PI_PORT, PI_BAUD\n",
    "import time"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "9e7647fa-fbbf-4c74-9243-b0d79fcfca71",
   "metadata": {},
   "source": [
    "## 连接机械臂"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "id": "c2b73e87-4110-45b2-8b93-01e3e06d0aeb",
   "metadata": {},
   "outputs": [],
   "source": [
    "mc = MyCobot(PI_PORT, PI_BAUD)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "cb3874df-6780-41be-8611-fbc3ee6fba1c",
   "metadata": {},
   "source": [
    "## 机械臂归零"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 17,
   "id": "05364661-bd01-4cb2-86cc-68d777712d5e",
   "metadata": {},
   "outputs": [],
   "source": [
    "mc.send_angles([0, 0, 0, 0, 0, 0], 50)\n",
    "time.sleep(2)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "0987a1df-0daa-4475-be1c-61d35bfa8f0f",
   "metadata": {},
   "source": [
    "## 放松机械臂，可以手动摆动机械臂\n",
    "\n",
    "注意机械臂会瞬间放松，要小心磕碰"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 25,
   "id": "4c5ad3d5-1e8f-4df5-a3f3-92a22ea9d60e",
   "metadata": {},
   "outputs": [],
   "source": [
    "mc.release_all_servos()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "3c9742cb-e955-47de-b6f3-8c085e33f647",
   "metadata": {},
   "source": [
    "## 获取角度"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 26,
   "id": "ec724dd8-ba80-4941-be61-6fdd077b8706",
   "metadata": {},
   "outputs": [],
   "source": [
    "angles = mc.get_angles()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 27,
   "id": "4df6928d-290d-4b96-9d5c-9c948dc5ad94",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "[14.94, -74.35, 4.57, 126.12, 2.46, -1.66]"
      ]
     },
     "execution_count": 27,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "angles"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "c5c86ce3-444d-4a5a-87b0-ae1bd5b67bb9",
   "metadata": {},
   "source": [
    "## 获取坐标"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 28,
   "id": "631b3ef7-edcb-4d5d-a675-c02f887f3fb8",
   "metadata": {},
   "outputs": [],
   "source": [
    "coords = mc.get_coords()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 29,
   "id": "f02f4b17-e824-4d05-a4c3-77e8badde886",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "[172.5, -18.8, 274.7, -33.26, -3.09, -75.21]"
      ]
     },
     "execution_count": 29,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "coords"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "351d460c-2590-43de-8e59-382ddeb4af51",
   "metadata": {},
   "source": [
    "## 让机械臂归零"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 35,
   "id": "995a63a3-b79b-4432-af2f-0757e10665c6",
   "metadata": {},
   "outputs": [],
   "source": [
    "mc.send_angles([0, 0, 0, 0, 0, 0], 50)\n",
    "time.sleep(2)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "650eea62-eb53-459b-a251-75d7578207bc",
   "metadata": {},
   "source": [
    "## 角度控制：让机械臂回到刚刚手动拖拽的位置"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 31,
   "id": "9876b933-b33a-4d27-b25d-98e3612c4714",
   "metadata": {},
   "outputs": [],
   "source": [
    "mc.send_angles(angles, 50)\n",
    "time.sleep(2)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 32,
   "id": "842455bf-efb5-445e-b2bd-3b51181a0cf3",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "[14.67, -74.97, 3.95, 125.59, 1.84, -1.14]"
      ]
     },
     "execution_count": 32,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "mc.get_angles()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "ec9ebd10-3b78-4ed8-a11c-a30f1908df0d",
   "metadata": {},
   "source": [
    "## 坐标控制：让机械臂回到刚刚手动拖拽的位置"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 33,
   "id": "90be0757-9e0b-484e-9eee-89af0c05b8df",
   "metadata": {},
   "outputs": [],
   "source": [
    "mc.send_coords(coords, 20, 0)\n",
    "time.sleep(2)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 34,
   "id": "ceaf89d7-f2c3-4ab3-84f6-cec412159391",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "[175.2, -18.8, 271.6, -34.42, -2.72, -75.26]"
      ]
     },
     "execution_count": 34,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "mc.get_coords()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "b8ede6e5-57ec-4d5e-b698-ff0729a53e2e",
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3 (ipykernel)",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.8.10"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}
